"""  
    需求:创建服务端，解析客户端提交的数据并响应结果
    流程：
        1.导包；
        2.初始化ROS2客户端；
        3.自定义节点类；
            3-1.创建服务端；
            3-2.编写回调函数处理请求并产生响应；        
        4.调用spain函数，并传入节点对象；
        5.资源释放。
"""

import rclpy
from rclpy.node import Node
from ardupilot_msgs.srv import ArmMotors

class ArmMotorsServer(Node):
    def __init__(self):
        super().__init__('arm_motors_server')
        # 创建服务并注册回调函数
        self.srv = self.create_service(ArmMotors, '/ap/arm_motors', self.arm_motors_callback)#(服务接口类型，话题名称，回调函数)
        self.get_logger().info("服务端已创建！(python)")


    def arm_motors_callback(self, request, response):#请求对象和响应对象
        # 这里可以根据请求的内容来设置是否执行 arm
        # 例如，假设你只是简单地发布 arm: True
        response.result = True  # 设置响应为 arm: True,响应应该为result，这里需要秋该
        self.get_logger().info(f"Arming motors: {response.result}")
        self.get_logger().info("服务端result发布true")
        return response

def main(args=None):
    rclpy.init(args=args)
    Armserver = ArmMotorsServer()
    rclpy.spin(Armserver)
    
    rclpy.shutdown()

if __name__ == '__main__':
    main()
